#include <math.h>
#include <stdbool.h>


typedef struct {
	
     double la;
     double lo;
     double alt;
     double cog;
     double sog;
	 int 	lastfix;
	 int   validdata;	
     } gps_data;



typedef struct {
     double la;
     double lo;
     } position;


typedef struct {
     double la;
     double lo;
     int alt;
     int arrival_dist ;
     } t_waypoint;

extern gps_data last_pos;

/********************************************

Navigational routines to be used in RC autopilot software


double radians(double deg) 
double degrees(double rad) 

unsigned int get_gps_dist(float flat1, float flon1, float flat2, float flon2)
int get_gps_course(float flat1, float flon1, float flat2, float flon2)


Simplified versions. OK precision under 10 km
int get_gps_course2(float flat1, float flon1, float flat2, float flon2)
unsigned int get_gps_dist2(float flat1, float flon1, float flat2, float flon2)
 


**********************************************/



double radians(double deg);

double degrees(double rad); 

double get_gps_course2(float flat1, float flon1, float flat2, float flon2);


double get_gps_dist2(float flat1, float flon1, float flat2, float flon2);



 /*************************************************************************
 //Function to calculate the course between two waypoints
 //I'm using the real formulas--no lookup table fakes!
 *************************************************************************/ 
 double get_gps_course(float flat1, float flon1, float flat2, float flon2);
 

 /*************************************************************************
 //Function to calculate the distance between two waypoints
 //I'm using the real formulas
 *************************************************************************/ 
  double get_gps_dist(float flat1, float flon1, float flat2, float flon2);
 
char getnextwp();
double getwpcourse();

/*************************************

wp_next 

moves on to next wp in list. if no more waypoints 
exists it starts over with number one. 

**************************************/

char wp_next(int next_wp); 


/*************************************

wp_goto 

goto wp number.wp_no

in wp_no <= number_of_waypoints then loads that wp
else returns false and do nothing

**************************************/

char wp_goto(int wp_no); 



/*************************************

wp_go_home

Activates wp[0] and returns 1
if wp[0] is not set then wp_go_home returns -1

**************************************/

char wp_go_home(); 



/*************************************

wp_add 

adds a waypoint to the waypoint list and return new number of waypoints

**************************************/

char wp_add(double la,double lo, int alt, int arrival_dist);


/*************************************

wp_set_home 

sets wp[0] to home coordinates

**************************************/

void wp_set_home(double la,double lo, int alt, int arrival_dist);
 

/*************************************

navigate 

calculate turnangle to follow a line frpn lastwp to nextwp

**************************************/

// double new_la,double new_lo,double new_cog,double new_sog

double navigate();

